/*
 * Kalman.cpp
 *
 *      Author: bohni
 *
 *      Letztes Update: 03.11.13 | Code aufgeräumt
 */


#include "Kalman.h"

///////////////////////////////////////////////////////////////


struct kalman {
	// p** = Fehlercovarianzmatrix
	// k*  = Kalmanverst�rkungsmatrix

	int32_t p00;
	int16_t p10;
	int16_t p01;
	int16_t p11;
	int32_t k0;
	int16_t k1;
	int16_t angle;
};

struct sensor_tmp {
	int16_t rate;
	int16_t bias;
	int16_t accel;
	int16_t angle_accel;
};

// Instanzen für X und Y Achse erstellen
struct kalman kalman_X = { 10, 0, 0, 10, 0 ,0 };
struct kalman kalman_Y = { 10, 0, 0, 10 ,0 ,0 };
struct sensor_tmp x_achse = { 0, 0, 0, 0 };
struct sensor_tmp y_achse = { 0, 0, 0, 0 };

// S = Estimate error
int32_t S = 0;

// y = Angle difference
int16_t y = 0;

int16_t angle_x = 0;
int16_t angle_y = 0;
int16_t angle_z = 0;

int16_t angle_v_z = 0;

int16_t accel_z = 0;

int32_t tmp = 0;

uint8_t q_angle = 0;
uint8_t q_gyro = 0;
uint8_t r_angle = 0;
uint8_t lowpass_filter = 0;

///////////////////////////////////////////

static void convert_raw_data();

////////////////////////////////////////////

void kalman_init() {
	/* EEPROM Adressen
	 *
	 *	mode	| 0x00
	 *
	 * anteil_p | 0x01
	 * anteil_i | 0x02
	 * anteil_d | 0x03
	 *
	 * q_angle 	| 0x05
	 * q_gyro  	| 0x06
	 * r_angle 	| 0x07
	 * lowPass	| 0x08
	 *
	 * p_z 		| 0x09
	 * i_z 		| 0x0A
	 * d_z 		| 0x0B
	 *
	 */

	x_achse.bias = xG_off / 131;
	y_achse.bias = yG_off / 131;

	x_achse.accel =  sensor_puffer[3] / 300;
	y_achse.accel =  sensor_puffer[4] / 300;
	accel_z =  sensor_puffer[5] / 300;


	// EEPROM Werte lesen

	q_angle = eeprom_read_byte( ( uint8_t * ) 0x05 );
	q_gyro	= eeprom_read_byte( ( uint8_t * ) 0x06 );
	r_angle = eeprom_read_byte( ( uint8_t * ) 0x07 );
	lowpass_filter = eeprom_read_byte( ( uint8_t * ) 0x08 );

}


void kalman_update() {

	convert_raw_data();


	// X-Achse

		kalman_X.angle +=  x_achse.rate * ABTASTZEIT_IN_MS / 10;		// 100 = 1� //passt genau wegen 10ms intervall

	// a-priori Fehler Kovarianz

		tmp = ABTASTZEIT_IN_MS * kalman_X.p11 / 1000;
		kalman_X.p01 -= tmp;
		kalman_X.p10 -= tmp;
		tmp = ABTASTZEIT_IN_MS * ( q_angle - kalman_X.p01 - kalman_X.p10 - tmp );
		kalman_X.p00 += tmp;
		tmp = ABTASTZEIT_IN_MS * q_gyro;
		kalman_X.p11 += tmp;

	// S = Estimate error
		S = kalman_X.p00 + r_angle;


		tmp = kalman_X.p00  * 1000 / S;
		kalman_X.k0 = tmp;
		tmp = kalman_X.p10  * 1000 / S;
		kalman_X.k1 = tmp;

		y = x_achse.angle_accel *100 - kalman_X.angle;

	// Update Angle and Bias
		tmp = kalman_X.k0 * y / 1000;
		kalman_X.angle += tmp;

		tmp = kalman_X.k1 * y / 1000;
		x_achse.bias += tmp / 100;

	// Update Fehler-Kovarianz

		tmp = kalman_X.k0 * kalman_X.p00 / 1000;
		kalman_X.p00 -= tmp;

		tmp = kalman_X.k0 * kalman_X.p01 / 1000;
		kalman_X.p01 -= tmp;

		tmp = kalman_X.k1 * kalman_X.p00  / 1000;
		kalman_X.p10  -= tmp;

		tmp = kalman_X.k1 * kalman_X.p01 / 1000;
		kalman_X.p11  -= tmp;

		angle_x = kalman_X.angle / 10;



	// Y-Achse:

		kalman_Y.angle +=  y_achse.rate * ABTASTZEIT_IN_MS / 10; 	// 100 = 1grad

	// a-priori Fehler Kovarianz

		tmp = ABTASTZEIT_IN_MS * kalman_Y.p11 / 1000;
		kalman_Y.p01 -= tmp;
		kalman_Y.p10 -= tmp;
		tmp = ABTASTZEIT_IN_MS * ( q_angle - kalman_Y.p01 - kalman_Y.p10 - tmp );
		kalman_Y.p00 += tmp;
		tmp = ABTASTZEIT_IN_MS * q_gyro;
		kalman_Y.p11 += tmp;

		S = kalman_Y.p00 + r_angle;

		tmp = kalman_Y.p00 * 1000 / S;
		kalman_Y.k0 = tmp;
		tmp = kalman_Y.p10 * 1000 / S;
		kalman_Y.k1 = tmp;


		y = y_achse.angle_accel * 100  - kalman_Y.angle;

	// Update Angle and Bias
		tmp =kalman_Y.k0 * y / 1000;
		kalman_Y.angle += tmp;

		tmp = kalman_Y.k1 * y / 1000;
		y_achse.bias += tmp / 100;

	// Update Fehler-Kovarianz
		tmp = kalman_Y.k0 * kalman_Y.p00 / 1000;
		kalman_Y.p00 -= tmp;
		tmp = kalman_Y.k0 * kalman_Y.p01 / 1000;
		kalman_Y.p01 -= tmp;
		tmp = kalman_Y.k1 * kalman_Y.p00 / 1000;
		kalman_Y.p10 -= tmp;
		tmp = kalman_Y.k1 * kalman_Y.p01 / 1000;
		kalman_Y.p11 -= tmp;


		angle_y = kalman_Y.angle /10;

}

static void convert_raw_data() {
	// Gyro range: 250grad/s
	// Gyro werte umrechnen
		x_achse.rate = sensor_puffer[0] / 131; // in grad/s
		y_achse.rate = sensor_puffer[1] / 131;

		x_achse.rate = x_achse.rate  - x_achse.bias;
		y_achse.rate = y_achse.rate  - y_achse.bias;

	// Accelerometer Werte Tiefpassfiltern

		x_achse.accel = x_achse.accel * lowpass_filter + sensor_puffer[3] / 300 *( 100 - lowpass_filter ) ;
		y_achse.accel = y_achse.accel * lowpass_filter + sensor_puffer[4] / 300 *( 100 - lowpass_filter ) ;
		x_achse.accel = x_achse.accel / 100;
		y_achse.accel = y_achse.accel / 100;
		accel_z = accel_z * lowpass_filter + sensor_puffer[5] / 300 *( 100 - lowpass_filter );
		accel_z = accel_z / 100;

	// Winkel über Accelerometer berechnen
		x_achse.angle_accel = arctan_deg( y_achse.accel, accel_z ); // in 1�

		y_achse.angle_accel = arctan_deg( x_achse.accel, accel_z );


	// ANGLE_Z :

	//angle_z += ( sensor_puffer[2] - zG_off ) / 131;  // 10 = 1°
		angle_v_z = ( sensor_puffer[2] - zG_off ) / 131;

#ifdef DEBUG_ENABLED
	//DEBUG Werte schreiben

		// ANGLE

		send_buffer[21] = kalman_X.angle / 100 + 128;
		send_buffer[22] = kalman_Y.angle / 100 + 128;
		send_buffer[23] = angle_z / 100 + 128;


		// ACCEL

		send_buffer[12] = x_achse.accel + 128;
		send_buffer[13] = y_achse.accel + 128;
		send_buffer[14] = accel_z + 128;

		// GYRO

		send_buffer[15] = sensor_puffer[0] / 131 + 128;
		send_buffer[16] = sensor_puffer[1] / 131 + 128;
		send_buffer[17] = sensor_puffer[2] / 131 + 128;
#endif
}

void kalman_setup_update() {

	system_pause();
	// Alte Kalman Einstellungen Senden

	send_buffer[0] = q_angle;
	send_buffer[1] = q_gyro;
	send_buffer[2] = r_angle;
	send_buffer[3] = lowpass_filter;

	UART_send(4);

//		while( !( UCSRA & (1<<UDRE)) );

//		UDR = q_angle;
//		while( !( UCSRA & ( 1 << UDRE ) ) );
//
//		UDR = q_gyro;
//		while( !( UCSRA & ( 1 << UDRE ) ) );
//
//		UDR = r_angle;
//		while( !( UCSRA & ( 1 << UDRE ) ) );
//
//		UDR = lowpass_filter;
//		while( !( UCSRA & ( 1 << UDRE ) ) );

		// Neue Reglerwerte Empfangen

		while ( !( UCSRA & ( 1 << RXC ) ) );
		q_angle = UDR;

		while ( !( UCSRA & ( 1 << RXC ) ) );
		q_gyro = UDR;

		while ( !( UCSRA & ( 1 << RXC ) ) );
		r_angle = UDR;

		while ( !( UCSRA & ( 1 << RXC ) ) );
		lowpass_filter = UDR;
		// Daten in EEPROM schreiben

		eeprom_update_byte( ( uint8_t *) 0x05, q_angle );
		eeprom_update_byte( ( uint8_t *) 0x06, q_gyro );
		eeprom_update_byte( ( uint8_t *) 0x07, r_angle );
		eeprom_update_byte( ( uint8_t *) 0x08, lowpass_filter );


		system_reset();

}



